/*
 * tarm_command.c
 *
 *  Created on: 2020年2月14日
 *      Author: AN
 */

#include "TARM_Middleware/tarm_math.h"
#include "TARM_Hardware/tarm_communication.h"

#include "TARM_Application/tarm_run.h"
#include "TARM_Application/tarm_init.h"

#include <rtthread.h>
#include <finsh_api.h>

#include <stdlib.h>
#include <stdio.h>

/************************************************************
*	指令解析
*	传入参数：	指令
*	返回值：	无
*	执行位置：
*	注释：
*	命令格式：
*	bug说明：
************************************************************/
//等同于memccpy去掉c，同时还把dest最后一位清位\0，返回不包含c字符的长度
int memccpy_rid(char *dest, char *src, unsigned char c, unsigned int count)
{
    char *dest_s;

    dest_s = strchr(src, c);
    if(dest_s == NULL)
        return -1;

    memcpy(dest, src, strlen(src) - strlen(dest_s));
    dest[strlen(src) - strlen(dest_s)] = '\0';

    return strlen(src) - strlen(dest_s);
}

//等同于strstr去掉str2
char* strstr_rid(const char * str1, const char * str2 )
{
    char* locate = strstr(str1,str2);
    if(locate==NULL)
        return NULL;

    locate += strlen(str2);
    return locate;
}

int search4(char *originate, char *command,double* command_num)    //定长指令
{
    int CommandNum = 3;
//    char command[3]="0";
//    double command_num[3]={0};
    char assis_str[30];
	int i;
    char *one = strstr_rid(originate, "_&");

    if(one == NULL)
        return -1;

    for(i=1;i<=CommandNum;i++)
    {
    	//获取命令
    	command[i-1] = one[0];
    	one += 1;

    	//获取参数字符串
    	memccpy_rid(assis_str, one, ';', strlen(one));
    	if(assis_str == NULL)
    		return i-1;


    	//转成参数
    	command_num[i-1] = atof(assis_str);

    	//移位
    	one += (strlen(assis_str)+1);

    	if(strrchr(one, ';') == NULL)
    		return i;

    }

    return 3;
}



#ifdef FINSH_USING_MSH
void TARM_MOVXY(int argc, char**argv)
{
	double x=0,y=0;
	int speed=0;
	char t_str[30];
	if(argc <= 2 || argc >= 5)
	{
		rt_kprintf("too little or too many arg!");
		return ;
	}
	x = atof(argv[1]);
	y = atof(argv[2]);
	if(argc==3)
	{
		speed=100;
	}
	else if(argc==4)
	{
		speed=atof(argv[3]);
	}
//	rt_kprintf("x-%f,y-%f,s-%f",x,y,speed);
	g_TARM_RunRecord.ReadyRunLoc_x = x;
	g_TARM_RunRecord.ReadyRunLoc_y = y;
	g_TARM_RunRecord.ReadyRunLoc_speed = speed;
	g_TARM_RunRecord.StartRun_pv =1;
	sprintf(t_str,"_&x:%f;\t_&y:%f;\n_&speed:%d;\r\r\n",x,y,speed);
	rt_kprintf(t_str);
}
MSH_CMD_EXPORT(TARM_MOVXY, mov with X Y speed : TARM_MOVXY 7.1 12.1 150.1);

void TARM_MOVR(int argc, char**argv)
{	
	double r=0;
	int speed=0;
	char t_str[30];
	if(argc <= 1 || argc >= 4)
	{
		rt_kprintf("too little or too many arg!");
		return ;
	}
	r = atof(argv[1]);
	if(argc==2)
	{
		speed=100;
	}
	else if(argc==3)
	{
		speed=atof(argv[2]);
	}
	g_TARM_RunRecord.ReadyRunLoc_r = r;
	g_TARM_RunRecord.ReadyRunLoc_speedr = speed;
	g_TARM_RunRecord.StartRun_r =1;
	sprintf(t_str,"_&r:%f;\n_&speed:%d;\r\r\n",r,speed);
	rt_kprintf(t_str);
}
MSH_CMD_EXPORT(TARM_MOVR, mov with X Y speed : TARM_MOVR 100 150.1);
#endif

#ifndef FINSH_USING_MSH_ONLY
void TARMF_MOVXY(double x, double y, int speed)
{
	char t_str[30];
	sprintf(t_str,"_&x:%f;\t_&y:%f;\n_&speed:%d;\r\r\n",x,y,speed);
	rt_kprintf(t_str);
}
FINSH_FUNCTION_EXPORT(TARMF_MOVXY, list thread);
#endif